#pragma config(Sensor, in1,    Gyro,                sensorLineFollower)
#pragma config(Sensor, in2,    L1,                  sensorLineFollower)
#pragma config(Sensor, in3,    M2,                  sensorLineFollower)
#pragma config(Sensor, in4,    R3,                  sensorLineFollower)
#pragma config(Sensor, dgtl1,  encR,                sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  encL,                sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  Righteye,            sensorSONAR_inch)
#pragma config(Sensor, dgtl7,  Lefteye,             sensorSONAR_inch)
#pragma config(Sensor, dgtl9,  Estop,               sensorTouch)
#pragma config(Sensor, dgtl10, Button,              sensorTouch)
#pragma config(Motor,  port2,           Right1,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           Right2,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           Right3,        tmotorNormal, openLoop)
#pragma config(Motor,  port5,           Left1,         tmotorNormal, openLoop)
#pragma config(Motor,  port6,           Left2,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           Left3,         tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Variables...
const bool Left = true;
const bool Right = false;
const int error = 7;

void clearGyro(int init)
{
  {
    SensorValue[Gyro] = sensorNone;
    wait1Msec(init);
    SensorValue[Gyro] = sensorGyro;
    wait1Msec(init);
  }
}
void testClearGyro()
{
  SensorValue[Gyro] = 0;
}
void Stop()
{
  motor[port2] = 0;
  motor[port3] = 0;
  motor[port4] = 0;
  motor[port5] = 0;
  motor[port6] = 0;
  motor[port7] = 0;
}



/*
1)This function allows you to turn using the Gyro.
2)enter a your how many degress you wants to turn and the power. Negative values turn Right.
3) Ex. Turn (45, -127)  ... turn Right
4) TODO: Make a Switch statement to tell which way to turn. and So I can add Error Fucn. also...
*/
void turn(int Degree, int Power,)
{
  clearGyro(100);

  while(SensorValue[Gyro] <= (abs(Degree*10)))//postitive values turn left.
  {
     motor[Right1] = Power;
     motor[Right2] = Power;
     motor[Right3] = Power;
     motor[Left1] = -Power;
     motor[Left2] = -Power;
     motor[Left3] = -Power;
  }
  Stop();
}

void turnAdvanced(int Degree, int Power, bool direction)
{
  testClearGyro();
  switch (direction){
  case Left:
    while(SensorValue[Gyro] <= (abs(Degree*10)))
    {
     motor[Right1] = Power;
     motor[Right2] = Power;
     motor[Right3] = Power;
     motor[Left1] = -Power;
     motor[Left2] = -Power;
     motor[Left3] = -Power;
    }
     motor[Right1] = 6;
     motor[Right2] = 6;
     motor[Right3] = 6;
     motor[Left1] = -6;
     motor[Left2] = -6;
     motor[Left3] = -6;

    while(SensorValue[Gyro] > (abs(Degree*10)) + error || SensorValue[Gyro] < (abs(Degree*10)) - error)
      if(abs(SensorValue[Gyro]) > (abs(Degree*10)))
      {
        motor[Right1] = -30;
     motor[Right2] = -30;
     motor[Right3] = -30;
     motor[Left1] = 30;
     motor[Left2] = 30;
     motor[Left3] = 30;
      }
      else
      {
         motor[Right1] = 30;
         motor[Right2] = 30;
         motor[Right3] = 30;
         motor[Left1] = -30;
         motor[Left2] = -30;
         motor[Left3] = -30;
      }

      Stop();
      break;
    case Right:
      while(SensorValue[Gyro] <= (abs(Degree*10)))
          {
              motor[Right1] = -Power;
              motor[Right2] = -Power;
              motor[Right3] = -Power;
              motor[Left1] = Power;
              motor[Left2] = Power;
              motor[Left3] = Power;
           }

      while(SensorValue[Gyro] > (abs(Degree*10)) + error || SensorValue[Gyro] < (abs(Degree*10)) - error){
        if(abs(SensorValue[Gyro]) > (abs(Degree*10)))
       {
        motor[Right1] = -30;
     motor[Right2] = -30;
     motor[Right3] = -30;
     motor[Left1] = 30;
     motor[Left2] = 30;
     motor[Left3] = 30;
      }
      else
      {
         motor[Right1] = 30;
         motor[Right2] = 30;
         motor[Right3] = 30;
         motor[Left1] = -30;
         motor[Left2] = -30;
         motor[Left3] = -30;
      }

        Stop();
        break;

      }//while






  }//switch
}




/*


motor[leftMotor] = 5;//eliminate drift...
motor[rightMotor] = -5;

while(SensorValue[Gyro] > (abs(Degree*10)) + error || SensorValue[Gyro] < (abs(Degree*10)) - error){
if(abs(SensorValue[Gyro]) > (abs(Degree*10)
{
motor[rightMotor] = -30;
motor[leftMotor] = 30;
}
else
{
motor[rightMotor] = 30;
motor[leftMotor] = -30;
}

Stop();
}

*/
